//
// Created by zzy on 22-5-22.
//
#include"AngleSolver.h"

namespace hnurm
{
    bool AngleSolver::Init()
    {
        _big_armor_real={cv::Point3f(-BIG_ARMOR_LEN / 2.0, BIG_ARMOR_WID / 2.0, 0.0),
                         cv::Point3f(BIG_ARMOR_LEN / 2.0, BIG_ARMOR_WID / 2.0, 0.0),
                         cv::Point3f(BIG_ARMOR_LEN / 2.0, -BIG_ARMOR_WID / 2.0, 0.0),
                         cv::Point3f(-BIG_ARMOR_LEN / 2.0, -BIG_ARMOR_WID / 2.0, 0.0)};

        _small_armor_real={cv::Point3f(-SMALL_ARMOR_LEN / 2.0, SMALL_ARMOR_WID / 2.0, 0.0),
                           cv::Point3f(SMALL_ARMOR_LEN / 2.0, SMALL_ARMOR_WID / 2.0, 0.0),
                           cv::Point3f(SMALL_ARMOR_LEN / 2.0, -SMALL_ARMOR_WID / 2.0, 0.0),
                           cv::Point3f(-SMALL_ARMOR_LEN / 2.0, -SMALL_ARMOR_WID / 2.0, 0.0)};

        _rotation_vect = cv::Mat::zeros(3, 1, CV_64FC1);
        _translation_vect = cv::Mat::zeros(3, 1, CV_64FC1);

        cv::FileStorage fs_read("/home/zzy/code_2022_season/together/AngleSolver/out_camera_data.xml", cv::FileStorage::READ);
        if (!fs_read.isOpened())
        {
            std::cerr << "failed to open CameraParams.xml" << std::endl;
            return false;
        }
        fs_read["camera_matrix"] >> _camera_matrix;
        fs_read["distortion_coefficients"] >> _distortion_matrix;
        fs_read.release();
        return true;
    }


    void AngleSolver::Solve(TargetInfo& target,float& distance,float& pitch,float& yaw)
    {
        //TODO: try some other method for solvePnP
        if(target._big_flag)
        {
            cv::solvePnP(_big_armor_real, target.vertexes, _camera_matrix, _distortion_matrix, _rotation_vect, _translation_vect, false, cv::SOLVEPNP_ITERATIVE);
        }

        if (!target._big_flag)
        {
            cv::solvePnP(_small_armor_real, target.vertexes, _camera_matrix, _distortion_matrix, _rotation_vect, _translation_vect, false, cv::SOLVEPNP_ITERATIVE);
        }

        distance = sqrt(pow(_translation_vect.at<double>(0, 0),2)
                        + pow(_translation_vect.at<double>(1, 0),2)
                        + pow(_translation_vect.at<double>(2, 0),2));

        yaw = atan(_translation_vect.at<double>(0, 0) / _translation_vect.at<double>(2, 0)) / CV_PI * 180;
        pitch = atan(_translation_vect.at<double>(1, 0) /  _translation_vect.at<double>(2, 0)) / CV_PI * 180;
    }


    void AngleSolver::CoordinatesTrans()
    {

    }


}//namespace hnurm
